function [x, P, Phi, GammaPQGammaPT] = kalmfiltextrap(x, P, F, GPQGPT, Tm, nPhi, fcnMTimes) %#codegen
%KALMFILTEXTRAP 卡尔曼滤波状态估计外推及误差协方差外推，采用普通的状态估计外推及误差协方差外推计算式
%
% Input Arguments:
% # x: m*1列向量，上一外推周期得到的状态向量
% # P: m*m矩阵，上一外推周期得到的误差协方差矩阵
% # F: m*m矩阵，动态矩阵F在外推周期内的均值
% # GPQGPT: m*m矩阵，GP*Q*GPT在外推周期内的均值,其中GP为过程噪声动态耦合矩阵，Q为过程噪声的功率谱密度矩阵，GPT为GP的转置
% # Tm: 标量，卡尔曼滤波外推周期间隔时间，单位s
% # nPhi: 标量，状态转移矩阵阶数
% # fcnMTimes: 函数句柄，用于计算本函数中的矩阵乘法，默认为普通乘法
%
% Output Arguments:
% # x: m*1列向量，本次外推周期得到的状态向量
% # P: m*m矩阵，本次外推周期得到的误差协方差矩阵
% # Phi: m*m矩阵，离散化之后的状态转移矩阵
% # GammaPQGammaPT: m*m矩阵，等价于GammaP*Q*GammaPT，其中GammaP为过程噪声耦合矩阵，Q为过程噪声序列协方差矩阵，GammaPT为GammaP的转置
%
% References:
% # 《应用导航算法工程基础》“误差协方差矩阵的外推算法”

if (nargin<7) || isempty(fcnMTimes)
    fcnMTimes = @kfextrapdefaultmtimes;
end
if nPhi < 1 % F为离散化的值，不用离散化
    Phi = F;
else
    Phi = eye(size(F)) + Tm*F;
    if nPhi >= 3
        FSquare = fcnMTimes(int8(KFExtrapMTimesFcnType.FF), F, F);
        Phi = Phi + (Tm^2)*FSquare/2 + (Tm^3)*(fcnMTimes(int8(KFExtrapMTimesFcnType.FSquareF), FSquare, F))/6;
    elseif nPhi == 2
        Phi = Phi + (Tm^2)*(fcnMTimes(int8(KFExtrapMTimesFcnType.FF), F, F))/2;
    end
end
GammaPQGammaPT = GPQGPT * Tm;
x = fcnMTimes(int8(KFExtrapMTimesFcnType.Phix), Phi, x);
% 分解成两步计算
tmp_PhiP = fcnMTimes(int8(KFExtrapMTimesFcnType.PhiP), Phi, P);
P = fcnMTimes(int8(KFExtrapMTimesFcnType.PhiPPhiT), tmp_PhiP, Phi') + GammaPQGammaPT; % REF1式12.56
end